"""
bat.py --

This file implements the Bat class.
Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.

"""

import vtk
import time
from numpy import *
from vtk.util.colors import *
from graphengine import axes
from graphengine import cindir
from graphengine import tire
from graphengine import cart
from graphengine import tube
from graphengine import cylinder
from systems import basesystem
from systems import link
from systems import dcmotor

class Bat( basesystem.BaseSystem):
	""" Bat system class definition"""

	#bat's geometric parameters - mts, degrees.
	#Cart's geometric parameters.

	CartX=0.1							#cart X
	CartY=0.025						#cart Y
	CartZ=0.04							#cart Z


	#Pole's geometric parameters.
	PendLength=0.3				#pendulum lenght
	PendRadius=0.0025		#pendulum radius

	#Tires
	TireLength = 0.0035
	TireRadius = PendRadius*2

	#bat world's geometric parameters - mts, degrees.

	#Table's parameters.
	BaseHeight = 0.35			#Base's height from the floor.
	TableLength= 2.0
	TableHeight= BaseHeight-0.05
	TableWidth= 0.5
	TableRadius = 0.015

	#Rail's parameters.
	RailX=2.1
	RailY=0.01
	RailZ=0.05

	#dynamic parameters - kg, mts.
	M=0.5					#M:     Cart mass
	m=0.2					#m:     pendulum mass
	K=0.0					#K:      Cart friction
	p=0.0					#p:      Pendulum friction
	grav = 9.8			#         world gravity.

	#dc-motor initialization.
	dcmotor = dcmotor.DCmotor('motor2')
	dcmotor.R = 10
	dcmotor.r  =  0.01
	dcmotor.Km =  1.0
	dcmotor.Kg =   0.02

	def cartobject(self, ren):
		""" This function builds the 3d object that represents the cart"""
		#Cart:
		#Geometric dimensions:length, height, width.
		x = self.CartX+0.1
		y = self.CartY+0.033
		z = self.CartZ+0.025
		cartgeomdim = [ x, y, z ]
		#Position and orientation: xpos, ypos, zpos, r,p,y.
		cartposorient = [ 0.0, 0.0, -(self.CartZ-0.01)/2, 0.0, 0.0, 90.0  ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cartlightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cartcolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		cartobject	=	cart.Cart( ren, cartgeomdim, cartposorient, cartlightprop, slate_grey )

		#Joint tool (made of three cylinders)
		JointOffset = -(y/2-0.01)
		cyl1lenght = 0.0125
		cyl1radius = 0.008
		#Geometrical dimensions (length, radius).
		cyl1geomdim = [ cyl1lenght, cyl1radius ]
		#Position and orientation. (xpos, ypos, zpos, r,p,y ).
		cyl1posorient = [ JointOffset, 0.0, -(z/2-0.005), 90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cyl1lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cyl1colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Cyl1Object	=	cylinder.Cylinder( ren, cyl1geomdim, cyl1posorient, cyl1lightprop, light_grey)

		cyl2lenght = 0.05
		cyl2radius = 0.001
		#Geometrical dimensions (length, radius).
		cyl2geomdim = [ cyl2lenght, cyl2radius ]
		#Position and orientation. (xpos, ypos, zpos, r,p,y ).
		cyl2posorient = [ JointOffset,  0.0, -(z/2+cyl1lenght+cyl2lenght/2-0.005), 90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cyl2lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cyl2colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Cyl2Object	=	cylinder.Cylinder( ren, cyl2geomdim, cyl2posorient, cyl2lightprop, light_grey)

		cyl3lenght = 0.03
		cyl3radius = 0.005
		#Geometrical dimensions (length, radius).
		cyl3geomdim = [ cyl3lenght, cyl3radius ]
		#Position and orientation. (xpos, ypos, zpos, r,p,y ).
		cyl3posorient = [ JointOffset,  0.0, -(z/2+cyl2lenght/2+cyl3lenght/2-0.008), 90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cyl3lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cyl3colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Cyl3Object	=	cylinder.Cylinder( ren, cyl3geomdim, cyl3posorient, cyl3lightprop, peacock )


		#setting the bat's tires geometric parameters.
		#bat's tires:
		#Geometric dimensions:length,radius.
		tiregeomdim = [ 2*self.TireLength, self.TireRadius+0.01]
		#Position and orientation: xpos, ypos, zpos, r,p,y.
		tire1posorient = [ -y+0.01, x/2-0.055, -z/2+0.014, 90.0, 0.0, 0.0 ]
		tire2posorient = [ -y+0.01, -x/2+0.055, -z/2+0.014, 90.0, 0.0, 0.0 ]
		tire3posorient = [ -y+0.01, x/2-0.055, z/2+0.021, 90.0, 0.0, 0.0 ]
		tire4posorient = [ -y+0.01, -x/2+0.055, z/2+0.021, 90.0, 0.0, 0.0 ]
		tire1 = tire.Tire( ren, tiregeomdim,  tire1posorient, mint, blue)
		tire2 = tire.Tire( ren, tiregeomdim,  tire2posorient, mint, blue)
		tire3 = tire.Tire( ren, tiregeomdim,  tire3posorient, mint, blue)
		tire4 = tire.Tire( ren, tiregeomdim,  tire4posorient, mint, blue)

		#Cart assembly
		assembly = vtk.vtkAssembly()
		assembly.SetPosition( 0.02,0.0,0.005 )
		assembly.AddPart( cartobject.cartActor )
		assembly.AddPart( self.Cyl1Object.cylinderActor )
		assembly.AddPart( self.Cyl2Object.cylinderActor )
		assembly.AddPart( self.Cyl3Object.cylinderActor )
		assembly.AddPart( tire1.tireassembly )
		assembly.AddPart( tire2.tireassembly )
		assembly.AddPart( tire3.tireassembly )
		assembly.AddPart( tire4.tireassembly )

		return assembly

	def draw( self, ren ):
		"""Renderizes bat on 3D screen"""

		#Base part of the main assembly - transformation axes.
		self.BaseAxes = axes.CreateAxes()
		#baseassembly.
		self.Baseassembly = vtk.vtkAssembly()
		self.Baseassembly.SetUserTransform( self.BaseTrans )
		self.Baseassembly.AddPart(self.BaseAxes)

		# Cart part of the assembly.
		#Transformation axes.
		self.CartAxes = axes.CreateAxes()
		#Associated 3D object.
		self.CartObject = self.cartobject( ren )
		#Link 1: Cartassembly.
		self.Link1assembly = vtk.vtkAssembly()
		self.Link1assembly.SetUserTransform( self.Link1.Trans )
		self.Link1assembly.AddPart( self.CartAxes )
		self.Link1assembly.AddPart( self.CartObject )

		#Pole part of the assembly.
		#transformation Axes
		self.PoleAxes = axes.CreateAxes()
		#Associated 3D object.
		#Tube:
		#dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		#Geometrical dimensions.
		tubegeomdim = [ 0.0, self.PendLength+0.013, 0.0, self.PendRadius+0.00125	]
		#Position and orientation.
		tubeposorient = [ 0.0,  0.0, 0.0, 0.0, 0.0, 90.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		tubelightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		tubecolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.PoleObject	=	tube.Tube( ren, tubegeomdim, tubeposorient, tubelightprop, light_grey )

		#a cylinder.
		cylalenght = 0.01
		cylaradius = 0.002
		#Geometrical dimensions (length, radius).
		cylageomdim = [ cylalenght, cylaradius ]
		#Position and orientation. (xpos, ypos, zpos, r,p,y ).
		cylaposorient = [ -(self.PendLength+0.01), 0.0, 0.0, 0.0, 0.0, 90.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cylalightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cylacolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.CylaObject	=	cylinder.Cylinder( ren, cylageomdim, cylaposorient, cylalightprop, light_grey)

		#Link 2: Poleassembly
		self.Link2assembly = vtk.vtkAssembly()
		self.Link2assembly.SetUserTransform( self.Link2.Trans )
		self.Link2assembly.AddPart(self.PoleAxes)
		self.Link2assembly.AddPart(self.PoleObject.tubeActor)
		self.Link2assembly.AddPart( self.CylaObject.cylinderActor )

		# Create bat's main assembly.
		self.Systemassembly = vtk.vtkAssembly()
		self.Systemassembly.AddPart(self.Baseassembly)
		self.Systemassembly.AddPart(self.Link1assembly)
		self.Systemassembly.AddPart(self.Link2assembly)

	def filldynsimulate(self):
		"""Fills 3D dynamic simulation values"""

		self.SimParam[0,:]=[self.M, self.m, self.PendLength, self.grav, self.K, self.p]

		#DCMOTOR           R    r     Km   Kg - motor 2
		if (self.dcmotor.type == 'motor2'):
			self.SimParam[1,:]=[self.dcmotor.R, self.dcmotor.r, self.dcmotor.Km, self.dcmotor.Kg, 0.0, 0.0 ]

